#include <stdio.h>
#include <stdlib.h>
#include <malloc.h>
#include "platform.h"
#include "xparameters.h"
#include "sleep.h"
#include "zrcar.h"
#include "xuartps.h"

#include "math.h"
#include "xgpiops.h"



struct Contact{
    u8 coord_x[3];
    int int_x;
    u8 coord_y[3];
    int int_y;
    u8 coord_name[3];
    struct Contact* next;
};




int main()
{
    /*********init*******/
    zrcar_bluetooth_init();
    //zrcar_gpio_init();
    int check=0;
    int i;
    unsigned char blue_data[2];
    u8 rxBuf=1;
    unsigned char choose[2];
    unsigned char ifgoon[2];
    unsigned char yesgoon[4]="goon";
    unsigned char notgoon[4]="over";
    unsigned char menu[4]={"menu"};
    // car_forward();


    //链表头节点
    struct Contact* head;
    struct Contact* p;
    head = (struct Contact*)malloc(sizeof(struct Contact));
    p = head;



    //MENU
    for(i = 0; i < 4 ; i++){
        zrcar_bluetooth_send(&menu[i],1);
        sleep(1);
    }
    //判断输入
    while(1){
        check=0;
        check = zrcar_bluetooth_recv(choose,2);
        sleep(1);
        if(check==1){
            zrcar_bluetooth_send(&choose[0],1);
            //zrcar_bluetooth_send(&choose[1],1);
            check=0;
            sleep(1);
            break;
        }
    }
///////////////////////////////////////////
//  //整体逻辑框架
//  //choose[0]使用choose[0]构建菜单
//  if(choose[0] == 1)
//  {
//      //扫描地图
//
//  }
//  else if(choose[0] == 2)
//  {
//      //坐标和目的地连接
//
//  }
//  else if(choose[0] == 3)
//  {
//      //输入指定目的地，计算最短路径并到达指定位置
//
//  }
////////////////////////////////////////////

    //坐标和目的地连接
    if(choose[0] == '2')
    {
        do{
            //新建节点
            p->next = (struct Contact*)malloc(sizeof(struct Contact));
            p=p->next;
            //坐标x
            while(1)
            {
                check = zrcar_bluetooth_recv(p->coord_x,3);
                if(check){
                    for(i = 0; i < check ; i++){
                        zrcar_bluetooth_send(&p->coord_x[i],1);
                        sleep(1);
                    }
                    p->int_x=(p->coord_x[0]-48)*100+(p->coord_x[1]-48)*10+(p->coord_x[2]-48);
                    xil_printf("int_x:%d",p->int_x);
                    check=0;
                    break;
                }
                sleep(1);

            }
            //坐标y
            while(1)
            {
                check = zrcar_bluetooth_recv(p->coord_y,3);
                if(check){
                    for(i = 0; i < check ; i++){
                        zrcar_bluetooth_send(&p->coord_y[i],1);
                        sleep(1);
                    }
                    p->int_y=(p->coord_y[0]-48)*100+(p->coord_y[1]-48)*10+(p->coord_y[2]-48);
                    xil_printf("int_y:%d",p->int_y);
                check=0;
                break;
                }
                sleep(1);
            }
            //名字
            while(1)
            {
                check = zrcar_bluetooth_recv(p->coord_name,3);
                if(check){
                    for(i = 0; i < check ; i++){
                        zrcar_bluetooth_send(&p->coord_name[i],1);
                        sleep(1);
                    }
                check=0;
                break;
                }
                sleep(1);
            }


            //是否继续
            while(1)
            {
                check = zrcar_bluetooth_recv(ifgoon,2);
                if(check)
                {
                    if(ifgoon[0]!='#')
                    {
                        for(i=0;i<4;i++)
                        {
                            zrcar_bluetooth_send(&yesgoon[i],1);
                            sleep(1);
                        }
                    }
                    else
                    {
                        for(i=0;i<4;i++)
                        {
                            zrcar_bluetooth_send(&notgoon[i],1);
                            sleep(1);
                        }
                    }
                    sleep(1);
                    check=0;
                    break;
                }
                sleep(1);
            }

        }while(ifgoon[0]!='#');


    }
//  while(1)
//  {
//      //while(flag == 0)  //receive until finished receiving a frame.
//      check = zrcar_bluetooth_recv(blue_data,2);
//      if(check){
//          for(i = 0; i < check ; i++){
//              zrcar_bluetooth_send(&blue_data[i],1);
//          }
//          check=0;
//      }
//      sleep(1);
//  }


    return 0;
}

